// CubicSpline.cpp
#include "CubicSpline.h"
#include <QDebug>

CubicSpline::CubicSpline(const QVector<double>& x, const QVector<double>& y)
    : m_x(x), m_y(y), m_n(x.size())
{
    Q_ASSERT_X(m_x.size()  == m_y.size(),  "CubicSpline",
               "X and Y vectors must have same size");
    Q_ASSERT_X(m_n >= 2, "CubicSpline",
               "At least two points required");

    std::sort(m_x.begin(),  m_x.end());
    calculateCoefficients();
}

double CubicSpline::interpolate(double x)
{
    if(!m_coeffCalculated) {
        qWarning() << "Coefficients not calculated";
        return std::numeric_limits<double>::quiet_NaN();
    }

    if(x < m_x.first()  || x > m_x.last())  {
        qWarning() << "Extrapolation not allowed";
        return std::numeric_limits<double>::quiet_NaN();
    }

    const int idx = findSegment(x);
    const double dx = x - m_x[idx];
    return m_a[idx] + m_b[idx]*dx + m_c[idx]*dx*dx + m_d[idx]*dx*dx*dx;
}

void CubicSpline::calculateCoefficients()
{
    // 系数内存预分配
    m_a.resize(m_n);
    m_b.resize(m_n);
    m_c.resize(m_n);
    m_d.resize(m_n);

    // 构建三对角矩阵
    Eigen::VectorXd h(m_n-1), alpha(m_n-1);
    for(int i=0; i<m_n-1; ++i){
        h[i] = m_x[i+1] - m_x[i];
        if(i > 0) {
            alpha[i] = 3*(m_a[i+1] - m_a[i])/h[i]
                       - 3*(m_a[i] - m_a[i-1])/h[i-1];
        }
    }

    // 设置边界条件（自然样条）
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(m_n, m_n);
    Eigen::VectorXd B = Eigen::VectorXd::Zero(m_n);
    A(0,0) = A(m_n-1, m_n-1) = 1.0;

    // 填充矩阵
    for(int i=1; i<m_n-1; ++i){
        A(i, i-1) = h[i-1];
        A(i, i) = 2*(h[i-1] + h[i]);
        A(i, i+1) = h[i];
        B(i) = alpha[i];
    }

    // 矩阵求解
    Eigen::VectorXd c = A.lu().solve(B);
    m_c = QVector<double>(c.data(),  c.data()  + c.size());

    // 计算最终系数
    for(int i=0; i<m_n-1; ++i){
        m_d[i] = (m_c[i+1] - m_c[i])/(3*h[i]);
        m_b[i] = (m_a[i+1] - m_a[i])/h[i]
                 - h[i]*(2*m_c[i] + m_c[i+1])/3;
    }

    m_coeffCalculated = true;
}

int CubicSpline::findSegment(double x) const
{
    auto it = std::upper_bound(m_x.begin(),  m_x.end(),  x);
    return std::max(0, std::min(int(it - m_x.begin())  - 1, m_n-2));
}
